
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sim_multicopter.c
  * @author     baiyang
  * @date       2022-11-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sim_multicopter.h"

#include <stdio.h>
#include <string.h>

#include <rtthread.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void sim_multicopter_calculate_forces(sim_multicopter_t multicopter, const struct sitl_input *input, Vector3f_t *rot_accel, Vector3f_t *body_accel);
static void sim_multicopter_update(sim_aircraft_t aircraft, const struct sitl_input *input);
/*----------------------------------variable----------------------------------*/
struct sim_aircraft_ops multicopter_ops = {.set_start_location = NULL,
                                           .set_interface_ports = NULL,
                                           .update = sim_multicopter_update,
                                           .set_config = NULL,
                                           .rangefinder_beam_width = NULL,
                                           .perpendicular_distance_to_rangefinder_surface = NULL,
                                           .on_ground = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sim_multicopter_ctor(sim_multicopter_t multicopter, const char *frame_str)
{
    memset(multicopter, 0, sizeof(struct sim_multicopter));

    sim_aircraft_ctor(&multicopter->aircraft, &multicopter_ops, frame_str);

    multicopter->frame = sim_frame_find_frame(frame_str);
    if (multicopter->frame == NULL) {
        printf("Frame '%s' not found", frame_str);
        return;
    }

    sim_frame_init(multicopter->frame, frame_str, &multicopter->aircraft.battery);

    multicopter->aircraft.mass = sim_frame_get_mass(multicopter->frame);
    multicopter->aircraft.frame_height = 0.1f;
    multicopter->aircraft.ground_behavior = GROUND_BEHAVIOR_NO_MOVEMENT;
    multicopter->aircraft.lock_step_scheduled = true;
}

/* static object creator */
sim_aircraft_t sim_multicopter_create(const char *frame_str)
{
    sim_multicopter_t multicopter = (sim_multicopter_t)rt_malloc(sizeof(struct sim_multicopter));

    if (!multicopter) {
        return NULL;
    }

    sim_multicopter_ctor(multicopter, frame_str);

    return (sim_aircraft_t)multicopter;
}

// calculate rotational and linear accelerations
static void sim_multicopter_calculate_forces(sim_multicopter_t multicopter, const struct sitl_input *input, Vector3f_t *rot_accel, Vector3f_t *body_accel)
{
    sim_aircraft_t aircraft = &multicopter->aircraft;

    aircraft->motor_mask |= ((1U<<multicopter->frame->num_motors)-1U) << multicopter->frame->motor_offset;
    sim_frame_calculate_forces(multicopter->frame , aircraft, input, rot_accel, body_accel, aircraft->rpm, true);

    sim_aircraft_add_shove_forces(aircraft, rot_accel, body_accel);
    sim_aircraft_add_twist_forces(aircraft, rot_accel);
}

/*
  update the multicopter simulation by one time step
 */
static void sim_multicopter_update(sim_aircraft_t aircraft, const struct sitl_input *input)
{
    sim_multicopter_t multicopter = (sim_multicopter_t)aircraft;

    // get wind vector setup
    sim_aircraft_update_wind(aircraft, input);

    Vector3f_t rot_accel = VECTOR3F_DEFAULT_VALUE;

    sim_multicopter_calculate_forces(multicopter, input, &rot_accel, &aircraft->accel_body);

    // estimate voltage and current
    sim_frame_current_and_voltage(multicopter->frame, &aircraft->battery_voltage, &aircraft->battery_current);

    sim_battery_set_current(&aircraft->battery, aircraft->battery_current);

    sim_aircraft_update_dynamics(aircraft, &rot_accel);
    sim_aircraft_update_external_payload(aircraft, input);

    // update lat/lon/altitude
    sim_aircraft_update_position(aircraft);
    sim_aircraft_time_advance(aircraft);

    // update magnetic field
    sim_aircraft_update_mag_field_bf(aircraft);
}
/*------------------------------------test------------------------------------*/


